#include "VoxelSpace.h"
#include "Visualizer.h"
#include "numeric.h"
#include <maya/MIntArray.h>

VoxelSpace::VoxelSpace(const BBox& box, int voxel_factor)
  : m_bbox(box)
  , m_vfactor(voxel_factor)
{
  m_inc = MVector((m_bbox.xmax-m_bbox.xmin) / m_vfactor,
    (m_bbox.ymax-m_bbox.ymin) / m_vfactor,
    (m_bbox.zmax-m_bbox.zmin) / m_vfactor);
  total_voxels_for_all_rays = 0;
  total_polygons_for_all_rays = 0;
  found_intersections_with_polygons = 0;
  m_diagLength = m_bbox.diagonal();  
}


VoxelSpace::~VoxelSpace(void)
{
}

MVector VoxelSpace::center() const
{
  MFloatVector ctr;
  m_bbox.center(ctr.x, ctr.y, ctr.z);
  return ctr;
}

void VoxelSpace::get_voxel_coord(const MFloatPoint& space_coord, Idx3& vox_coord) const
{
	int x,y,z;
	x = (space_coord.x/space_coord.w - m_bbox.xmin)/m_inc.x;
	if(x == m_vfactor)
		x--;
	y = (space_coord.y/space_coord.w - m_bbox.ymin)/m_inc.y;
	if(y == m_vfactor)
		y--;
	z = (space_coord.z/space_coord.w - m_bbox.zmin)/m_inc.z;
	if(z == m_vfactor)
		z--;
	vox_coord.x = x;
	vox_coord.y = y;
	vox_coord.z = z;
}

int VoxelSpace::get_voxel_idx(const Idx3& vox_coord) const
{
	int z,y,x;
	z = vox_coord.z;
	y = vox_coord.y;
	x = vox_coord.x;
  return (z * (m_vfactor)*(m_vfactor)) + 
    (y * (m_vfactor)) + 
    x;
}

Idx3 VoxelSpace::idx_3d_from_idx(int idx) const
{
  Idx3 coord;
  coord.x = idx % m_vfactor;
  idx /= m_vfactor;
  coord.y = idx % m_vfactor;
  idx /= m_vfactor;
  coord.z = idx % m_vfactor;
  return coord;
}

void VoxelSpace::init(std::vector<std::vector<triangle>> triangles)
{
	for(int k = 0 ; k < m_vfactor ; k++)
	{
		for(int j = 0 ; j < m_vfactor ; j++)
		{
			for(int i = 0 ; i < m_vfactor ; i++)
			{
				std::vector<triangle_object> vec;
	
				//define this voxel
				BBox box;
				box.xmin = i*m_inc.x+m_bbox.xmin;
				box.ymin = j*m_inc.y+m_bbox.ymin;
				box.zmin = k*m_inc.z+m_bbox.zmin;
				box.xmax = i*m_inc.x+m_bbox.xmin + m_inc.x;
				box.ymax = j*m_inc.y+m_bbox.ymin + m_inc.y;
				box.zmax = k*m_inc.z+m_bbox.zmin + m_inc.z;

				for(int m = 0 ; m < triangles.size() ; m++)
				{
					for(int t = 0 ; t < triangles.at(m).size() ; t++)
					{
						float box_center[3];
						box.center(box_center[0], box_center[1], box_center[2]);
						float boxhalfsize[3];
						boxhalfsize[0] = abs(box.xmax-box.xmin)/2;
						boxhalfsize[1] = abs(box.ymax-box.ymin)/2;
						boxhalfsize[2] = abs(box.zmax-box.zmin)/2;
						float triverts[3][3];
						triverts[0][0] = triangles.at(m).at(t).p1.x/triangles.at(m).at(t).p1.w; 
						triverts[0][1] = triangles.at(m).at(t).p1.y/triangles.at(m).at(t).p1.w; 
						triverts[0][2] = triangles.at(m).at(t).p1.z/triangles.at(m).at(t).p1.w; 
						triverts[1][0] = triangles.at(m).at(t).p2.x/triangles.at(m).at(t).p2.w; 
						triverts[1][1] = triangles.at(m).at(t).p2.y/triangles.at(m).at(t).p2.w; 
						triverts[1][2] = triangles.at(m).at(t).p2.z/triangles.at(m).at(t).p2.w; 
						triverts[2][0] = triangles.at(m).at(t).p3.x/triangles.at(m).at(t).p3.w; 
						triverts[2][1] = triangles.at(m).at(t).p3.y/triangles.at(m).at(t).p3.w; 
						triverts[2][2] = triangles.at(m).at(t).p3.z/triangles.at(m).at(t).p3.w; 
						int tri_intersects_box = 0;
						tri_intersects_box = triBoxOverlap(box_center,boxhalfsize,triverts);
						if(tri_intersects_box > 0)
						{
							triangle_object to;
							to.t.p1 = triangles.at(m).at(t).p1;
							to.t.p2 = triangles.at(m).at(t).p2;
							to.t.p3 = triangles.at(m).at(t).p3;
							to.object = m;
							to.index = t;
							vec.push_back(to);
						}
					}
				}
				tri_inside_vox.push_back(vec);
			}
		}
	}
}

#define voxel2x(a) (a*m_inc.x+m_bbox.xmin)
#define voxel2y(a) (a*m_inc.y+m_bbox.ymin)
#define voxel2z(a) (a*m_inc.z+m_bbox.zmin)
#define x2voxel(a) ((a-m_bbox.xmin)/m_inc.x)
#define y2voxel(a) ((a-m_bbox.ymin)/m_inc.y)
#define z2voxel(a) ((a-m_bbox.zmin)/m_inc.z)


void VoxelSpace::floatAlgo(MFloatPoint point1, MFloatPoint point2, MVector ray_, std::vector<Idx3>& points, std::vector<int>& indices,   
    MFloatVector& ray_scaled, MFloatPoint& pt1_scaled, MFloatPoint& pt2_scaled)
{	
  points.clear();
  indices.clear();

  Idx3 p0, p1;
  get_voxel_coord(point1, p0);
  get_voxel_coord(point2, p1);

	int px,py,pz;
	float q,dx,dy,dz,sx,sy,sz,x,y,z,lx,ly,lz;
  float x1,y1,z1;
  float xscale = m_vfactor / (m_bbox.xmax-m_bbox.xmin);
  float yscale = m_vfactor / (m_bbox.ymax-m_bbox.ymin);
  float zscale = m_vfactor / (m_bbox.zmax-m_bbox.zmin);

  // move everything to origin by subtracting (xmin,ymin,zmin)
	x = point1.x - m_bbox.xmin; 
  y = point1.y - m_bbox.ymin; 
  z = point1.z - m_bbox.zmin;
  x1 = point2.x - m_bbox.xmin;
  y1 = point2.y - m_bbox.ymin;
  z1 = point2.z - m_bbox.zmin;

  // scale everything to make voxels 1x1x1
  MVector dray_scaled (ray_.x*xscale, ray_.y*yscale, ray_.z*zscale);
  MFloatVector ray = MFloatVector(dray_scaled.x, dray_scaled.y, dray_scaled.z);
  ray.normalize();
  ray_scaled = ray;

  MFloatPoint curr = MFloatPoint(x*xscale, y*yscale, z*zscale);
  x = curr.x;
  y = curr.y;
  z = curr.z;
  pt1_scaled = MFloatPoint(x, y, z);
  MFloatPoint curr1 = MFloatPoint(x1*xscale, y1*yscale, z1*zscale);
  x1 = curr1.x;
  y1 = curr1.y;
  z1 = curr1.z;
  pt2_scaled = MFloatPoint(x1, y1, z1);

	if(NearlyEq(ray.x, 0))
	{
		lx = FLT_MAX;
		sx = 0;
	}
	else
	{
    float testx = sqrt(ray.x*ray.x + ray.y*ray.y + ray.z*ray.z);
		dx =  1.0f / ray.x;
		//dx = ray.x/abs(ray.x);
		if(ray.x < 0)
		{
			lx = dx*(x-floor(x));
      if(NearlyEq(lx,0)){
        lx = dx;
      }
			px = -1;
			sx = dx*x;
		}
		else
		{
			lx = dx*(floor(x)+1-x);
			px = 1;
			sx = dx*(m_vfactor-x);
		}
	}

	if(NearlyEq(ray.y, 0))
	{
		ly = FLT_MAX;
		sy = 0;
	}
	else
	{
    float testy = sqrt(ray.x*ray.x + ray.y*ray.y + ray.z*ray.z);
		dy = 1.0f / ray.y;
		//dy = ray.y/abs(ray.y);
		if(ray.y < 0)
		{
			ly = dy*(y-floor(y));
      if(NearlyEq(ly,0)){
        ly = dy;
      }

			py = -1*m_vfactor;
			sy = dy*y;
		}
		else
		{
			ly = dy*(floor(y)+1-y);
			py = 1*m_vfactor;
			sy = dy*(m_vfactor-y);
		}
	}

	if(NearlyEq(ray.z, 0))
	{
		lz = FLT_MAX;
		sz = 0;
	}
	else
	{
    float testz = sqrt(ray.x*ray.x + ray.y*ray.y + ray.z*ray.z);
		dz = 1.0f / ray.z;
		//dz = ray.z/abs(ray.z);
		if(ray.z < 0)
		{
			lz = dz*(z-floor(z));
      if(NearlyEq(lz,0)){
        lz = dz;
      }
			pz = -m_vfactor*m_vfactor;
			sz = dz*z;
		}
		else
		{
			lz = dz*(floor(z)+1-z);
			pz = m_vfactor*m_vfactor;
			sz = dz*(m_vfactor-z);
		}
	}

	Idx3 p(p0.x,p0.y,p0.z);
	int index = 0;
	int starting_index = get_voxel_idx(p);
	index += starting_index;

	while(1)
	{
		if(index < 0)
			return;
		indices.push_back(index);
    p = idx_3d_from_idx(index);
    points.push_back(p);

		if(p.x == p1.x && p.y == p1.y && p.z == p1.z)
			break;
		
		if(abs(lx) <= abs(ly) && abs(lx) <= abs(lz))
		{
			if(abs(lx) >= abs(sx))
				return;
			index += px;
			lx += dx;
		}
		else
		{
			if(abs(ly) <= abs(lx) && abs(ly) <= abs(lz))
			{
				if(abs(ly) >= abs(sy))
					return;
				index += py;
				ly += dy;
			}
			else
			{
				if(abs(lz) <= abs(lx) && abs(lz) <= abs(ly))
				{
					if(abs(lz) >= abs(sz))
						return;
					index += pz;
					lz += dz;
				}
				else break;
			}
		}
	}
}
	
void VoxelSpace::Bresenham3D(Idx3 p0, Idx3 p1, std::vector<Idx3>& points)
{
	int Cont, dx, dy, dz, Adx, Ady, Adz, x_inc, y_inc, z_inc,
        err_1, err_2, dx2, dy2, dz2, xxx, yyy, zzz, Xold, Yold, Zold,
        Xnew, Ynew, Znew;

	Xold = p0.x;
	Yold = p0.y;
	Zold = p0.z;

	Idx3 p(Xold,Yold,Zold);
	points.push_back(p);

	Xnew = p1.x;
	Ynew = p1.y;
	Znew = p1.z;

	dx = Xnew - Xold;
	dy = Ynew - Yold;
	dz = Znew - Zold;

  	if (dx < 0) 
		x_inc = -1;
	else
		x_inc =  1;
   
	if (dy < 0) 
		y_inc = -1;
	else
		y_inc =  1;
    
	if(dz < 0) 
		z_inc = -1;
	else
		z_inc =  1;

    Adx = abs(dx);
	Ady = abs(dy);
    Adz = abs(dz);
	
	dx2 = Adx*2;
    dy2 = Ady*2;
	dz2 = Adz*2;

	while (!(Xold == Xnew && Yold == Ynew && Zold == Znew))
	{
		xxx = Xold;
		yyy = Yold;
		zzz = Zold;
   
		if((Adx>= Ady)&&(Adx>= Adz)) 
		{
			err_1 = dy2 - Adx;
			err_2 = dz2 - Adx;
      
			for(Cont = 0 ; Cont <= Adx-1 ; Cont++)
			{
				/*if(err_1 > 0) 
				{
					yyy += y_inc;
					err_1 -= dx2;
				} 
         
				if(err_2 > 0)  
				{
					zzz += z_inc;
					err_2 -= dx2;
				} */
         
				 err_1 += dy2;
				 err_2 += dz2;
				 xxx += x_inc;

				 // visit
				 Idx3 p(xxx,yyy,zzz);
				 points.push_back(p);
			}
		}
   
		if((Ady> Adx) && (Ady>= Adz))
		{
			 err_1 = dx2 - Ady;
			 err_2 = dz2 - Ady;
		
			for(Cont = 0 ; Cont <= Ady-1 ; Cont++)
			{
				/*if(err_1 > 0) 
				{
					xxx += x_inc;
					err_1 -= dy2;
				}
         
				if(err_2 > 0)
				{
					zzz+= z_inc;
					err_2 -= dy2;
				}*/
        
				err_1 += dx2;
				err_2 += dz2;
				yyy+= y_inc;

				//visit
				Idx3 p(xxx,yyy,zzz);
				points.push_back(p);
			}
		}
   
		if((Adz > Adx) && (Adz > Ady)) 
		{
			err_1 = dy2 - Adz;
			err_2 = dx2 - Adz;
      
			for(Cont = 0 ; Cont <= Adz-1 ; Cont++)
			{  
				/*if(err_1 > 0) 
				{
					yyy += y_inc;
					err_1 -= dz2;
				}
         
				if(err_2 > 0)
				{
					xxx += x_inc;
					err_2 -= dz2;
				}*/
         
				err_1 += dy2;
				err_2 += dx2;
				zzz += z_inc;  

				//visit
				Idx3 p(xxx,yyy,zzz);
				points.push_back(p);
			}   
		}

		Xold = Xnew;
		Yold = Ynew;
		Zold = Znew;
	}
}

bool VoxelSpace::intersectsTriangle(MFloatPoint& p0, MVector& r, MFloatPoint& p1, MFloatPoint& p2, MFloatPoint& p3, MFloatPoint& p, float& mult)
{
	float normal[3],minusnormal[3],ac[3],ab[3],bc[3], ca[3];
	ac[0] = p3.x/p3.w-p1.x/p1.w; ac[1] = p3.y/p3.w-p1.y/p1.w; ac[2] = p3.z/p3.w-p1.z/p1.w;
	ab[0] = p2.x/p2.w-p1.x/p1.w; ab[1] = p2.y/p2.w-p1.y/p1.w; ab[2] = p2.z/p2.w-p1.z/p1.w;
	bc[0] = p3.x/p3.w-p2.x/p2.w; bc[1] = p3.y/p3.w-p2.y/p2.w; bc[2] = p3.z/p3.w-p2.z/p2.w;
	ca[0] = p1.x/p1.w-p3.x/p3.w; ca[1] = p1.y/p1.w-p3.y/p3.w; ca[2] = p1.z/p1.w-p3.z/p3.w;
	CROSS(normal,ac,ab);
	if (DOT(normal,r) == 0) return false; // they are parallel so they don't intersect !
	minusnormal[0] = -normal[0]; minusnormal[1] = -normal[1]; minusnormal[2] = -normal[2];
	float d = DOT(normal,p1);
	d = -1*d;
	float t_i = -1*((DOT(normal,p0)+d))/DOT(normal,r);
	if(t_i < 0)
		return false;
	p.x = p0.x + t_i*r.x;
	p.y = p0.y + t_i*r.y;
	p.z = p0.z + t_i*r.z;
	p.w = 1;
	mult = t_i;
	MVector AP(p.x-p1.x/p1.w, p.y-p1.y/p1.w, p.z-p1.z/p1.w);
	MVector BP(p.x-p2.x/p2.w, p.y-p2.y/p2.w, p.z-p2.z/p2.w);
	MVector CP(p.x-p3.x/p3.w, p.y-p3.y/p3.w, p.z-p3.z/p3.w);
	MVector cross1, cross2, cross3;
	CROSS(cross1,AP,ab);
	CROSS(cross2,BP,bc);
	CROSS(cross3,CP,ca);
	/*if(DOT(normal,cross1) < 0 && DOT(normal,cross2) < 0 && DOT(normal,cross3) < 0)
	return true;
	if(DOT(normal,cross1) > 0 && DOT(normal,cross2) > 0 && DOT(normal,cross3) > 0)
	return true;*/
	float check1 = DOT(normal,cross1);
	float check2 = DOT(normal,cross2);
	float check3 = DOT(normal,cross3);

	if(DOT(normal,cross1) < 0)
		return false;
	if(DOT(normal,cross2) < 0)
		return false;
	if(DOT(normal,cross3) < 0)
		return false;
	return true;
	//return false;
}


bool VoxelSpace::ourColsestIntersection(MVector rayDir, MFloatPoint raySource, float& minHitRayParam, int& minMesh, int& minHitTriangle, MFloatPoint& minHitPoint, float& Bary1, float& Bary2, float& Bary3)
{
	bool returnedValue = false;
	minHitRayParam = FLT_MAX;
	float inear,ifar;
    bool ray_intersects_bb = RayCubeIntersection(rayDir, raySource, inear, ifar);
	MFloatPoint savep0,savep1,savep2;

	if(ray_intersects_bb)
	{
		MFloatPoint pt1 = raySource + inear*rayDir;
        MFloatPoint pt2 = raySource + ifar*rayDir;
		std::vector<Idx3> points;
        std::vector<int> indices;
        MFloatVector ray_sc;
        MFloatPoint pt1_sc;
        MFloatPoint pt2_sc;
        floatAlgo(pt1, pt2, rayDir, points, indices, ray_sc, pt1_sc, pt2_sc);
		for(int i = 0 ; i < indices.size() ; i++)
		{
			int index = indices[i];
			bool intersects = false;
			total_voxels_for_all_rays++;
			for(int j = 0 ; j < tri_inside_vox.at(index).size() ; j++)
			{
				total_polygons_for_all_rays++;
				triangle_object to;
				to.object = tri_inside_vox[index][j].object;
				to.t.p1 = tri_inside_vox[index][j].t.p1;
				to.t.p2 = tri_inside_vox[index][j].t.p2;
				to.t.p3 = tri_inside_vox[index][j].t.p3;
				to.index = tri_inside_vox[index][j].index;
				MFloatPoint intersection_point;
				float mult;
				intersects = intersectsTriangle(raySource, rayDir, to.t.p1, to.t.p2, to.t.p3, intersection_point, mult);
				if(intersects)
				{
					found_intersections_with_polygons++;
					returnedValue = true;
					if(mult < minHitRayParam)
					{
						minHitRayParam = mult;
						minHitTriangle = to.index;
						minMesh = to.object;
						savep0 = to.t.p1; savep1 = to.t.p2; savep2 = to.t.p3;
					}
				}
			}

		}
    if(returnedValue)
    {
      minHitPoint = raySource + minHitRayParam*rayDir;
      MFloatVector v1 = savep2 - savep1;
      MFloatVector v2 = savep2 - savep0;
      MFloatVector v5 = savep0 - savep2;
      MFloatVector v3 = minHitPoint - savep1;
      MFloatVector v4 = minHitPoint - savep2;
      MFloatVector vn;
      CROSS(vn,v1,v2);
      float area = vn.length();
				MFloatVector normal = vn/area;
      normal /= area;
      MFloatVector c1,c2;
				CROSS(c1,v1,v3);
      CROSS(c2,v5,v4)
      Bary1 = -1*DOT(c1,normal);
      Bary2 = -1*DOT(c2,normal);
      Bary3 = 1.0 - Bary1 - Bary2;
      return true;
    } 
	}
	return false;
}

bool VoxelSpace::RayCubeIntersection(MVector dir,MFloatPoint p0, float& tnear, float& tfar)
{
	tnear = -FLT_MAX; tfar = FLT_MAX;
	float t1,t2,temp,tCube;
	bool intersectFlag = true;
	float d[3],p[3],b1[3],b2[3];
	b1[0] = m_bbox.xmin; b1[1] = m_bbox.ymin; b1[2] = m_bbox.zmin; //min corner
	b2[0] = m_bbox.xmax; b2[1] = m_bbox.ymax; b2[2] = m_bbox.zmax; // max corner
	d[0] = dir.x; d[1] = dir.y; d[2] = dir.z; // direction
	p[0] = p0.x/p0.w; p[1] = p0.y/p0.w; p[2] = p0.z/p0.w; // starting point

	for(int i = 0 ;i < 3; i++)
	{
		if(d[i] == 0)
		{
			if(p[i] < b1[i] || p[i] > b2[i])
				intersectFlag = false;
		}
		else
		{
			t1 = (b1[i] - p[i])/d[i];
			t2 = (b2[i] - p[i])/d[i];
			if(t1 > t2)
			{
				temp = t1;
				t1 = t2;
				t2 = temp;
			}
			if(t1 > tnear)
				tnear = t1;
			if(t2 < tfar)
				tfar = t2;
			if(tnear > tfar)
				return false;
			if(tfar < 0)
				return false;
		}
	}
	return intersectFlag;
 }